close all; clear; clc;

%% base
px = 0.1; py = 0.2; pz = 0.3;
rz = 0.004; ry = 0.005; rx = 0.006;

T_b0 = Translx(px) * Transly(py) * Translz(pz) * Trotz(rz) * Troty(ry) * Trotx(rx);

%% tool
tx = 0.1; ty = 0.2; tz = 0.3;
T_tool = Transl(tx, ty, tz);

%% joint
q1 = 0.1; q2 = 0.2; q3 = 0.3; q4 = 0.4; q5 = 0.5; q6 = 0.6;

%%
alpha1 = 0; alpha2 = pi/2; alpha3 = 0; alpha4 = pi/2; alpha5 = -pi/2; alpha6 = pi/2;
a1 = 0; a5 = 0; a6 = 0;
d2 = 0; d5 = 0;
a2 = 0.18; a3 = 0.58; a4 = 0.16;
d1 = 0.43; d3 = 0.0; d4 = 0.6395; d6 = 0.115;
theta1 = 0; theta2 = pi/2; theta3 = 0; theta4 = 0; theta5 = 0; theta6 = 0;
sigma = 1; q = 0;

Alpha = [alpha1, alpha2, alpha3, alpha4, alpha5, alpha6];
A = [a1, a2, a3, a4, a5, a6];
D = [d1, d2, d3, d4, d5, d6];
Theta = [theta1, theta2, theta3, theta4, theta5, theta6];

DH_para.Alpha = Alpha;
DH_para.A = A;
DH_para.D = D;
DH_para.Theta = Theta;

T01 = TransformationMatrixMDH(alpha1, a1, d1, theta1, sigma, q1);
T12 = TransformationMatrixMDH(alpha2, a2, d2, theta2, sigma, q2);
T23 = TransformationMatrixMDH(alpha3, a3, d3, theta3, sigma, q3);
T34 = TransformationMatrixMDH(alpha4, a4, d4, theta4, sigma, q4);
T45 = TransformationMatrixMDH(alpha5, a5, d5, theta5, sigma, q5);
T56 = TransformationMatrixMDH(alpha6, a6, d6, theta6, sigma, q6);
T06 = T01 * T12 * T23 * T34 * T45 * T56;

%%
q_delta = [0, 0, 0, 0, 0, 0, 0, 0, 0];
real_q_base = [px, py, pz, rz, ry, rx, tx, ty, tz];
q = [q1, q2, q3, q4, q5, q6];
Jacobian = calculate_Jacobian(DH_para, q_delta, q);

real_T = T_b0 * T06 * T_tool;
delta_p = real_T(1:3, 4) - T06(1:3, 4);

